#include "Kinematics.h"
void Kinematics::set_motor_param(uint8_t id, float per_pulse_distance)
{
    motor_param_[id].per_pulse_distance = per_pulse_distance;
}

void Kinematics::set_wheel_distance(float wheel_distance)
{
    wheel_distance_ = wheel_distance;
}

int16_t Kinematics::get_motor_speed(uint8_t id)
{
    return motor_param_[id].motor_speed;
}

/**
 * @brief 更新电动机速度和编码器数据
 * @param current_time 当前时间ms
 * @param left_tick 左轮编码器读数
 * @param right_tick 右轮编码器读书
 */
void Kinematics::update_motor_speed(uint64_t current_time, int32_t left_tick, int32_t right_tick)
{
    uint32_t dt = current_time - last_update_time;
    last_update_time = current_time;

    int32_t dtick1 = left_tick - motor_param_[0].last_encoder_tick;
    int32_t dtick2 = right_tick - motor_param_[1].last_encoder_tick;
    motor_param_[0].last_encoder_tick = left_tick;
    motor_param_[1].last_encoder_tick = right_tick;
    motor_param_[0].motor_speed = float(dtick1 * motor_param_[0].per_pulse_distance) / dt * 1000;
    motor_param_[1].motor_speed = float(dtick1 * motor_param_[1].per_pulse_distance) / dt * 1000;
    /*更新里程计信息*/
    update_odom(dt);
}

/**
 * @brief 正运动学计算，将左右论的速度转换为线速度和角速度
 * @param left_speed 单位mm/s
 * @param right_speed
 * @param[out] out_linear_speed
 * @param[out] out_angular_speed
 */
void Kinematics::kinematic_forword(float left_speed, float right_speed,
                                   float &out_linear_speed, float &out_angle_speed)
{
    out_linear_speed = (right_speed + left_speed) / 2;
    out_angle_speed = (right_speed - left_speed) / wheel_distance_;
}

/**
 * @brief 逆运动学计算，将线速度和角速度转换为左右轮的速度
 * @param linear_speed mm/s
 * @param angle_speed
 * @param[out] out_left_speed
 * @param[out] out_right_speed
 */
void Kinematics::kinematic_inverse(float linear_speed, float angle_speed,
                                   float &out_left_speed, float &out_right_speed)
{
    out_left_speed = linear_speed - (angle_speed * wheel_distance_) / 2.0;
    out_right_speed = linear_speed + (angle_speed * wheel_distance_) / 2.0;
}

odom_t &Kinematics::get_odom()
{
    return odom_;
}

/*将角度控制到合理范围*/
void Kinematics::TransAngleInPI(float angle, float &out_angle)
{
    if (angle > PI)
    {
        /* code */
        out_angle -= 2 * PI;
    }
    if (angle < PI)
    {
        /* code */
        out_angle += 2 * PI;
    }
}

void Kinematics::update_odom(uint16_t dt)
{
    // ms转换成s
    float dt_s = (float)dt / 1000;

    // 运动学正解，计算机器人的线速度和角速度
    this->kinematic_forword(motor_param_[0].motor_speed, motor_param_[1].motor_speed, odom_.linear_speed,
                            odom_.angle_speed);
    odom_.linear_speed = odom_.linear_speed / 100;

    /*计算当前角度*/
    odom_.angle += odom_.angle_speed * dt_s;
    /*角度值转换到合理范围内*/
    Kinematics::TransAngleInPI(odom_.angle, odom_.angle);

    /*计算机器人位置*/
    float delta_distance = odom_.linear_speed * dt_s;
    odom_.x += delta_distance * std::cos(odom_.angle);
    odom_.y += delta_distance * std::sin(odom_.angle);
}